function P=AVP_pos_ang_func(G,P,k)
P.pos=[P.Vars(1),0];
P.ang=P.Vars(3);
